#include "Marking1.h"
#include "skill/Factory.h"
#include "TaskMediator.h"
#include "WorldDefine.h"
#include "VisionModule.h"
#include "staticparams.h"
#include "parammanager.h"
#include "KickStatus.h"
#include "game_state.h"
#include "RefereeBoxIf.h"
#include <bits/stdc++.h>
#include <fstream>

namespace  {
   bool DEBUG_SWITCH;
   const CGeoPoint OUR_GOAL(-PARAM::Field::PITCH_LENGTH/2,0);
   const CGeoPoint LEFT_GOAL_POST(-PARAM::Field::PITCH_LENGTH/2,-PARAM::Field::GOAL_WIDTH/2 - 2);
   const CGeoPoint RIGHT_GOAL_POST(-PARAM::Field::PITCH_LENGTH/2,PARAM::Field::GOAL_WIDTH/2 + 2);
   const CGeoSegment GOAL_LINE(LEFT_GOAL_POST,RIGHT_GOAL_POST);
   const static int MIN_X = -PARAM::Field::PITCH_LENGTH/2;

   bool theirtendourgoal(double dir,const CGeoPoint&pos)
   {
        double leftdir = (LEFT_GOAL_POST - pos).dir();
        double rightdir= (RIGHT_GOAL_POST - pos).dir();
        if(pos.y()>LEFT_GOAL_POST.y()&&pos.y()<RIGHT_GOAL_POST.y())
        {
            if(dir>rightdir||dir<leftdir)
                return true;
            else
                return false;
        }
        else
        {
            if(dir>rightdir&&dir<leftdir)
                return true;
            else
                return false;
        }
   }
   bool candiredefence(ifstream &fr1,int &total1)
   {
       int store1[6];
       fr1.open("Marking1.txt");
       fr1>>store1[0];
       fr1.close();
       fr1.open("Marking2.txt");
       fr1>>store1[1];
       fr1.close();
       fr1.open("Marking3.txt");
       fr1>>store1[2];
       fr1.close();
       fr1.open("Marking4.txt");
       fr1>>store1[3];
       fr1.close();
       fr1.open("Marking5.txt");
       fr1>>store1[4];
       fr1.close();
       if(total1==1 || total1==2)
       {
            return true;
       }
//       else if(total1==2)
//       {
//           if(store1[0]!=store1[1])
//           {
//                return true;
//           }
//       }
       else if(total1==3)
       {
           if(store1[0]!=store1[1])//&&store1[0]!=store1[2])
           {
                return true;
           }
       }
       else if(total1==4)
       {
           if(store1[0]!=store1[1]&&store1[0]!=store1[2])//&&store1[0]!=store1[3])
           {
                return true;
           }
       }
       else if(total1==5)
       {
           if(store1[0]!=store1[1]&&store1[0]!=store1[2]&&store1[0]!=store1[3])//&&store1[0]!=store1[4])
           {
                return true;
           }
       }
       return false;
   }
}

CMarking1::CMarking1(){
     ZSS::ZParamManager::instance()->loadParam(DEBUG_SWITCH, "DEBUG/CMarking1", false);
}
void CMarking1::plan(const CVisionModule* pVision){
    if(pVision->getCycle() - _lastCycle > PARAM::Vision::FRAME_RATE*0.1){
    }
    const int vecNumber = task().executor;
    const PlayerVisionT& self = pVision->ourPlayer(vecNumber);
    const auto mouse = self.Pos() + Utils::Polar2Vector(PARAM::Vehicle::V2::PLAYER_CENTER_TO_BALL_CENTER,self.Dir());
    auto taskFlag = task().player.flag;
    auto ball = pVision->ball();
    CGeoRectangle stand_rectangle(-PARAM::Field::PITCH_LENGTH/2 + PARAM::Field::PENALTY_AREA_DEPTH + PARAM::Field::MAX_PLAYER_SIZE/2+30,-PARAM::Field::PENALTY_AREA_WIDTH/2 - PARAM::Field::MAX_PLAYER_SIZE/2 - 30,-PARAM::Field::PITCH_LENGTH/2,PARAM::Field::PENALTY_AREA_WIDTH/2 + PARAM::Field::MAX_PLAYER_SIZE/2 + 30);
    CGeoPoint stand_pos,stand_pos1;

    CGeoPoint TargetPos;
    double TargetDir;
    int attack[6],total = 0;

    ofstream fw;
    ifstream fr;
    int store[6],realnum = -1;
    fr.open("Marking1.txt");
    fr>>store[0];
    fr.close();
    fr.open("Marking2.txt");
    fr>>store[1];
    fr.close();
    fr.open("Marking3.txt");
    fr>>store[2];
    fr.close();
    fr.open("Marking4.txt");
    fr>>store[3];
    fr.close();
    fr.open("Marking5.txt");
    fr>>store[4];
    //fw.close();
    fr.close();
    for(int num = 0;num<PARAM::Field::MAX_PLAYER-1;num++)
    {
        const PlayerVisionT &enemy = pVision->theirPlayer(num);
        if(enemy.Valid())
        {
            if(enemy.X()<PARAM::Field::PITCH_LENGTH/8)
            {
                attack[total] = num;
                total++;
            }
        }
    }

//    realnum = attack[0];
    if(store[0]<0||store[0]>16)
    {
        fw.open("Marking1.txt");
        fw<<attack[0];
        fw.close();
        realnum = attack[0];
//        const PlayerVisionT &enemy = pVision->theirPlayer(store);
//        if(attack[0]!=store)
//        {
//            if(enemy.X() < PARAM::Field::PITCH_LENGTH/8)
//            {
//                realnum = store;
//            }
//            else
//            {
//                if(total==1)
//                {
//                    realnum = attack[0];
//                }
//                else if(total==2)
//                {

//                }
//            }
//        }
    }
    else
    {
        const PlayerVisionT &enemy = pVision->theirPlayer(store[0]);
        //cout<<store[0]<<endl;
        if(enemy.X() < PARAM::Field::PITCH_LENGTH/8 && candiredefence(fr,total)&&enemy.Valid())
        {
            realnum = store[0];
        }
        else
        {
            if(total==1)
            {
                realnum = attack[0];
                fw.open("Marking1.txt");
                fw<<attack[0];
                fw.close();
            }
            else if(total==2)
            {
                if(store[1] >= 0 && store[1]<16)
                {
                    for(int num = 0;num<2;num++)
                    {
                        if(store[1]!=attack[num])
                        {
                            realnum = attack[num];
                            fw.open("Marking1.txt");
                            fw<<attack[num];
                            fw.close();
                        }
                    }
                }
                else
                {
                    realnum = attack[0];
                    fw.open("Marking1.txt");
                    fw<<attack[0];
                    fw.close();
                }
            }
            else if(total == 3)
            {
                if(store[1] >= 0 && store[1]<16 && store[2] >= 0 && store[2]<16)
                {
                    for(int num = 0;num<3;num++)
                    {
                        if(store[1]==attack[num] || store[2]==attack[num])
                        {
                            continue;
                        }
                        realnum = attack[num];
                        fw.open("Marking1.txt");
                        fw<<attack[num];
                        fw.close();
                    }
                }
                else
                {
                    realnum = attack[0];
                    fw.open("Marking1.txt");
                    fw<<attack[0];
                    fw.close();
                }
            }
            else if(total == 4)
            {
                if(store[1] >= 0 && store[1]<16 && store[2] >= 0 && store[2]<16 && store[3] >= 0 && store[3]<16)
                {
                    for(int num = 0;num<4;num++)
                    {
                        if(store[1]==attack[num] || store[2]==attack[num] || store[3]==attack[num])
                        {
                            continue;
                        }
                        realnum = attack[num];
                        fw.open("Marking1.txt");
                        fw<<attack[num];
                        fw.close();
                    }
                }
                else
                {
                    realnum = attack[0];
                    fw.open("Marking1.txt");
                    fw<<attack[0];
                    fw.close();
                }
            }
            else
            {
                if(store[1] >= 0 && store[1]<16 && store[2] >= 0 && store[2]<16 && store[3] >= 0 && store[3]<16 && store[4] >= 0 && store[4]<16)
                {
                    for(int num = 0;num<5;num++)
                    {
                        if(store[1]==attack[num] || store[2]==attack[num] || store[3]==attack[num] || store[4]==attack[num])
                        {
                            continue;
                        }
                        realnum = attack[num];
                        fw.open("Marking1.txt");
                        fw<<attack[num];
                        fw.close();
                    }
                }
                else
                {
                    realnum = attack[0];
                    fw.open("Marking1.txt");
                    fw<<attack[0];
                    fw.close();
                }
            }
        }
    }
//cout<<store[1]<<endl;
    fw.close();
    fr.close();

    double ball_vel_dir = ball.Vel().dir();
    double d = PARAM::Math::PI;

    CWorldModel p;
    int time = p.InfraredOnCount(vecNumber);
    bool need_clear = time>3 && (self.Pos() - ball.Pos()).mod() < 105 && abs((ball.Pos() - self.Pos()).dir() - self.Dir())<d/6;//清球
    CGeoPoint clear_pos = ball.Pos() + Utils::Polar2Vector(90,0);//清球点

    CGeoSegment ball_line(ball.Pos(),ball.Pos() + Utils::Polar2Vector(99999,ball_vel_dir));
    CGeoLineLineIntersection danger_intersection1(ball_line, CGeoLine(self.Pos(),ball_vel_dir + d/2));//求机器人最近防守点
    CGeoLineLineIntersection danger_intersection2(ball_line, CGeoLine(self.Pos(),ball_vel_dir - d/2));//同上
    CGeoLineLineIntersection danger_intersection(ball_line,GOAL_LINE);
    bool danger_to_our_goal = danger_intersection.Intersectant() && ball_line.IsPointOnLineOnSegment(danger_intersection.IntersectPoint()) && GOAL_LINE.IsPointOnLineOnSegment(danger_intersection.IntersectPoint()) && (ball_vel_dir - (danger_intersection.IntersectPoint() - ball.Pos()).dir()) < PARAM::Math::PI/18;

    const PlayerVisionT&enemy = pVision->theirPlayer(realnum);
    if(danger_to_our_goal&&abs(enemy.Dir() - (ball.Pos() - enemy.Pos()).dir()) < d/15 && ball.Vel().mod() > 300){//紧急防球
        DribbleStatus::Instance()->setDribbleCommand(vecNumber, 3);
        if(ball_vel_dir > d/2)
        {

            if(ball_vel_dir>d/6*5)
            {
                TargetPos = ball_line.projection(mouse) + Utils::Polar2Vector(PARAM::Vehicle::V2::PLAYER_CENTER_TO_BALL_CENTER,self.Dir()+d);
                TargetDir = 0;
            }
            else
            {
                TargetDir = ball_vel_dir + PARAM::Math::PI;
                TargetPos = danger_intersection2.IntersectPoint();
            }
        }
        else
        {
            if(ball_vel_dir<-d/6*5)
            {
                TargetDir = 0;
                TargetPos = ball_line.projection(mouse) + Utils::Polar2Vector(PARAM::Vehicle::V2::PLAYER_CENTER_TO_BALL_CENTER,self.Dir()+d);
            }
            else
            {
                TargetDir = ball_vel_dir + PARAM::Math::PI;
                TargetPos = danger_intersection1.IntersectPoint();
            }
        }
    }
    else if(need_clear)
    {
        DribbleStatus::Instance()->setDribbleCommand(vecNumber,3);
        TargetDir = 0;
        TargetPos = clear_pos;
        KickStatus::Instance()->setKick(vecNumber,6400);
    }
    else
    {
            if(theirtendourgoal(enemy.Dir(),enemy.Pos()))
            {
                CGeoLineRectangleIntersection stand_intersection(CGeoLine(enemy.Pos(),enemy.Dir()),stand_rectangle);
                if(stand_intersection.intersectant())
                {
                  if(enemy.Pos().dist2(stand_intersection.point1())<enemy.Pos().dist2(stand_intersection.point2()))
                      stand_pos = stand_intersection.point1();
                  else
                      stand_pos = stand_intersection.point2();
                  if(stand_pos.x() < MIN_X + PARAM::Vehicle::V2::PLAYER_SIZE)
                      stand_pos.setX(MIN_X + PARAM::Vehicle::V2::PLAYER_SIZE);
                }
                else
                {
                    stand_pos = CGeoPoint(-PARAM::Field::PITCH_LENGTH/2 + PARAM::Field::MAX_PLAYER_SIZE/2 + PARAM::Field::PENALTY_AREA_DEPTH,0);
                }
                TargetDir = (enemy.Pos() - self.Pos()).dir();
                TargetPos = stand_pos;
            }
            else
            {
                if(Utils::Normalize(enemy.Dir() - (OUR_GOAL - enemy.Pos()).dir()) < 0)
                {
                    CGeoLineRectangleIntersection stand_intersection(CGeoLine(enemy.Pos(),RIGHT_GOAL_POST),stand_rectangle);
                    if(stand_intersection.intersectant())
                    {
                      if(enemy.Pos().dist2(stand_intersection.point1())<enemy.Pos().dist2(stand_intersection.point2()))
                          stand_pos = stand_intersection.point1();
                      else
                          stand_pos = stand_intersection.point2();
                      if(stand_pos.x() < MIN_X + PARAM::Vehicle::V2::PLAYER_SIZE)
                          stand_pos.setX(MIN_X + PARAM::Vehicle::V2::PLAYER_SIZE);
                    }
                    else
                    {
                        stand_pos = CGeoPoint(-PARAM::Field::PITCH_LENGTH/2 + PARAM::Field::MAX_PLAYER_SIZE/2 + PARAM::Field::PENALTY_AREA_DEPTH,0);
                    }
                }
                else if(Utils::Normalize(enemy.Dir() - (OUR_GOAL - enemy.Pos()).dir()) >0)
                {
                    CGeoLineRectangleIntersection stand_intersection(CGeoLine(enemy.Pos(),LEFT_GOAL_POST),stand_rectangle);
                    if(stand_intersection.intersectant())
                    {
                      if(enemy.Pos().dist2(stand_intersection.point1())<enemy.Pos().dist2(stand_intersection.point2()))
                          stand_pos = stand_intersection.point1();
                      else
                          stand_pos = stand_intersection.point2();
                      if(stand_pos.x() < MIN_X + PARAM::Vehicle::V2::PLAYER_SIZE)
                          stand_pos.setX(MIN_X + PARAM::Vehicle::V2::PLAYER_SIZE);
                    }
                    else
                    {
                        stand_pos = CGeoPoint(-PARAM::Field::PITCH_LENGTH/2 + PARAM::Field::MAX_PLAYER_SIZE/2 + PARAM::Field::PENALTY_AREA_DEPTH,0);
                    }
                }
                else
                {
                    CGeoLineRectangleIntersection stand_intersection(CGeoLine(enemy.Pos(),OUR_GOAL),stand_rectangle);
                    if(stand_intersection.intersectant())
                    {
                      if(enemy.Pos().dist2(stand_intersection.point1())<enemy.Pos().dist2(stand_intersection.point2()))
                          stand_pos = stand_intersection.point1();
                      else
                          stand_pos = stand_intersection.point2();
                      if(stand_pos.x() < MIN_X + PARAM::Vehicle::V2::PLAYER_SIZE)
                          stand_pos.setX(MIN_X + PARAM::Vehicle::V2::PLAYER_SIZE);
                    }
                    else
                    {
                        stand_pos = CGeoPoint(-PARAM::Field::PITCH_LENGTH/2 + PARAM::Field::MAX_PLAYER_SIZE/2 + PARAM::Field::PENALTY_AREA_DEPTH,0);
                    }
                }
            }
            TargetDir = (enemy.Pos() - self.Pos()).dir();
            TargetPos = stand_pos;
        }

    if((self.Pos() - TargetPos).mod() > PARAM::Vehicle::V2::PLAYER_SIZE*1)
    {
        taskFlag = PlayerStatus::ALLOW_DSS;
    }
    else
    {
        taskFlag = PlayerStatus::NOT_AVOID_OUR_VEHICLE;
    }

    if((self.Pos() - TargetPos).mod() > 700|| self.X() > -PARAM::Field::PITCH_LENGTH/2 + PARAM::Field::PENALTY_AREA_DEPTH + 700)
    {
       taskFlag = PlayerStatus::ALLOW_DSS + PlayerStatus::DODGE_BALL;
    }

//    if((self.X()>-PARAM::Field::PITCH_LENGTH/2+PARAM::Field::PENALTY_AREA_DEPTH-200)&&abs(self.Y())>(PARAM::Field::PENALTY_AREA_WIDTH/2-200))
//    {
//        taskFlag |= PlayerStatus::NOT_AVOID_PENALTY;
//    }
    taskFlag |= PlayerStatus::ALLOW_DSS;
    TaskT newTask(task());
    newTask.player.flag = taskFlag;
    newTask.player.pos = TargetPos;
    newTask.player.angle = TargetDir;
//newTask.player.pos = CGeoPoint(100,0);

    setSubTask(TaskFactoryV2::Instance()->SmartGotoPosition(newTask));
    //setSubTask(PlayerRole::makeItSmartGoto(vecNumber,CGeoPoint(100,100),0,0));
    _lastCycle = pVision->getCycle();
    CPlayerTask::plan(pVision);
}
